package com.lego.jbv;

import lejos.nxt.*;

public class Car 
{
    // Commands for the motors
    private final static int forward  = 1,
                             backward = 2,
                             stop     = 3;
	                         
    private static MotorPort leftMotor = MotorPort.B;
    private static MotorPort rightMotor = MotorPort.C;
    private static NXTRegulatedMotor scannerMotor;
    private static int scannerAngle;
    private static boolean isScanning = false;
    private static int scannerIniPos = 1000;
	
    private Car()
    {	   
    	
    } 
   
    public static void stop() 
    {
	    leftMotor.controlMotor(0,stop);
	    rightMotor.controlMotor(0,stop);
    }
   
    public static void forward(int leftPower, int rightPower)
    {
	    leftMotor.controlMotor(leftPower,forward);
	    rightMotor.controlMotor(rightPower,forward);
    }
   
    public static void backward(int leftPower, int rightPower)
    {
	    leftMotor.controlMotor(leftPower,backward);
	    rightMotor.controlMotor(rightPower,backward);
    }
    
    public static int counter() 
    {
        return ( (leftMotor.getTachoCount()+rightMotor.getTachoCount())/2);
    }
    
    public static void drive(int leftPower, int rightPower)
    {
    	if(0 <= leftPower)
    	{
    		leftMotor.controlMotor(leftPower, forward);
    	}
    	
    	else
    	{
    		leftMotor.controlMotor(-leftPower, backward);
    	}
    	
    	if(0 <= rightPower)
    	{
    		rightMotor.controlMotor(rightPower, forward);
    	}
    	
    	else
    	{
    		rightMotor.controlMotor(-rightPower, backward);
    	}
    }
    
    public static void scanner(int angle, int degreesPrSecond, NXTRegulatedMotor motor)
    {
    	scannerAngle = angle;
    	scannerMotor = motor;
    	
    	if(scannerIniPos == 1000)
    		scannerIniPos = scannerMotor.getPosition();
    	else
    	{
    		scannerMotor.setSpeed(360);
        	scannerMotor.rotateTo(scannerIniPos);
    	}
    	
    	scannerMotor.waitComplete();
    	scannerMotor.setSpeed(degreesPrSecond);
    	
    	Thread scannerThread = new Thread(new Runnable() {
			@Override
			public void run() {
				char state = 0;
				isScanning = true;
				
				while(isScanning)
				{	
					if(++state%2 > 0)
						scannerMotor.rotateTo(scannerAngle);
					else
						scannerMotor.rotateTo(-scannerAngle);
				}
			}
		});
    	
    	scannerThread.setDaemon(true);
    	scannerThread.start();
    }
    
    public static void stopScanner()
    {
    	isScanning = false;    	
    	scannerMotor.stop();
    }
    
    public static int getScannerPosition()
    {
    	return scannerMotor.getPosition();
    }
}
